#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <assert.h>
#include "cv.h"
#include "highgui.h"

#define SIG_DISP_H 150
#define SIG_DISP_W 400

int main() {
	int32_t key = -1;

	IplImage* MOV[4000];
	int32_t currFrame = 0;
	/* VIDEO DISPLAY */ {
		cvNamedWindow("video", CV_WINDOW_AUTOSIZE);

		CvCapture* VID = cvCaptureFromFile("../VID_0.avi"); {
			for (int i = 0; i < 4000; i++) {
				IplImage* a = cvQueryFrame(VID);
				MOV[i] = cvCloneImage(a);
			}
		}
		cvReleaseCapture(&VID);

		cvCreateTrackbar("frame", "video", &currFrame, 4000, NULL);
	}


	// SIGNAL DISPLAY
	cvNamedWindow("signal", CV_WINDOW_AUTOSIZE);
	IplImage* SIG = cvCreateImage(cvSize(SIG_DISP_W*2,SIG_DISP_H*3), IPL_DEPTH_8U, 3);
	double L_X[4000];
	double L_Y[4000];
	double L_Z[4000];
	double R_X[4000];
	double R_Y[4000];
	double R_Z[4000];
	/* IMPORT SIGNAL DATA */ {
		FILE* FL_X = fopen("../L_X_0.dat", "r");
		FILE* FL_Y = fopen("../L_Y_0.dat", "r");
		FILE* FL_Z = fopen("../L_Z_0.dat", "r");
		FILE* FR_X = fopen("../R_X_0.dat", "r");
		FILE* FR_Y = fopen("../R_Y_0.dat", "r");
		FILE* FR_Z = fopen("../R_Z_0.dat", "r");
		assert(FL_X);
		for (int i = 0; i < 4000; i++) {
			fscanf(FL_X, "%lf\n", &L_X[i]);
			fscanf(FL_Y, "%lf\n", &L_Y[i]);
			fscanf(FL_Z, "%lf\n", &L_Z[i]);
			fscanf(FR_X, "%lf\n", &R_X[i]);
			fscanf(FR_Y, "%lf\n", &R_Y[i]);
			fscanf(FR_Z, "%lf\n", &R_Z[i]);
		}
		fclose(FL_X);
		fclose(FL_Y);
		fclose(FL_Z);
		fclose(FR_X);
		fclose(FR_Y);
		fclose(FR_Z);
	}

	// DISPLAY LOOP
	while (key < 0) {
		cvShowImage("video", MOV[currFrame]);

		/* SHOW SIGNALS */ {
			cvSetZero(SIG);
			/* BORDERS */ {
				CvScalar white = cvScalar(255,255,255,255);
				int thickness = 1;
				int linetype = 8;
				cvLine(SIG, 
					cvPoint(0,SIG_DISP_H),
					cvPoint(2*SIG_DISP_W,SIG_DISP_H),
					white,
					thickness,
					linetype,
					0
					);
				cvLine(SIG, 
					cvPoint(0,2*SIG_DISP_H),
					cvPoint(2*SIG_DISP_W,2*SIG_DISP_H),
					white,
					thickness,
					linetype,
					0
					);
				cvLine(SIG, 
					cvPoint(SIG_DISP_W,0),
					cvPoint(SIG_DISP_W,SIG_DISP_H*3),
					white,
					thickness,
					linetype,
					0
					);
			}
			/* SIGNALS */ {
				CvScalar green = cvScalar(0,190,0,255);
				int thickness = 1;
				int linetype = 8;
				void drawLine(int offset_X, int offset_Y, double* buf, int idx) {
					int 	xpos0, ypos0,
						xpos1, ypos1;
					xpos0 = (int)(((float)idx/4000) * SIG_DISP_W)
						+ offset_X*SIG_DISP_W;
					xpos1 = (int)(((float)(idx + 1)/4000) * SIG_DISP_W)
						+ offset_X*SIG_DISP_W;
					ypos0 = (int)( (-buf[idx+0]/(5.0)*SIG_DISP_H + SIG_DISP_H*0.5) )
						+ offset_Y*SIG_DISP_H;
					ypos1 = (int)( (-buf[idx+1]/(5.0)*SIG_DISP_H + SIG_DISP_H*0.5) )
						+ offset_Y*SIG_DISP_H;
					cvLine(SIG, 
						cvPoint(xpos0,ypos0),
						cvPoint(xpos1,ypos1),
						green,
						thickness,
						linetype,
						0
						);
					return;
				}
				for (int i = 0; i < 4000-1; i++) {
					drawLine(0,0,L_X,i);
					drawLine(0,1,L_Y,i);
					drawLine(0,2,L_Z,i);
					drawLine(1,0,R_X,i);
					drawLine(1,1,R_Y,i);
					drawLine(1,2,R_Z,i);
				}
			}
			/* HIGHLIGHTING */ {
				CvScalar yellow = cvScalar(0,255,255,255);
				int thickness = 1;
				int linetype = 8;

				int posxL = (int)(((float)currFrame/4000 + 0)*SIG_DISP_W);
				int posxR = (int)(((float)currFrame/4000 + 1)*SIG_DISP_W);
				
				cvLine(SIG, 
					cvPoint(posxL,0),
					cvPoint(posxL,3*SIG_DISP_H),
					yellow,
					thickness,
					linetype,
					0
					);
				cvLine(SIG, 
					cvPoint(posxR,0),
					cvPoint(posxR,3*SIG_DISP_H),
					yellow,
					thickness,
					linetype,
					0
					);
			}
			/* TEXT */ {
				CvScalar color = cvScalar (111,190,111,255);
				CvFont font;
				cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX,
					0.5f, 0.5f, 0, 1, 8);
				int font_H = 15;
				int font_W = 5;
				void write(int offset_x, int offset_y, const char* words) {
					int posx, posy;
					posx = offset_x*SIG_DISP_W + font_W;
					posy = offset_y*SIG_DISP_H + font_H;
					cvPutText(SIG, words, cvPoint(posx,posy), &font, color);
				}
				write(0,0,"left X");
				write(0,1,"left Y");
				write(0,2,"left Z");
				write(1,0,"right X");
				write(1,1,"right Y");
				write(1,2,"right Z");
			}
			cvShowImage("signal", SIG);
		}

		key = cvWaitKey(1);
	}

	// EXIT
	cvDestroyWindow("display");
	return 0;
}

